#include "screen.h"
#include "codeports.h"

void
SCR_Init(void)
{
	unsigned int i;

	if((cp.video.descriptor = open("/dev/fb0", O_RDWR)) == -1)
	{
		perror("Error: Unable to open framebuffer device /dev/fb0");
		exit(1);
	}

	ioctl(cp.video.descriptor, FBIOGET_FSCREENINFO, &cp.video.mode.fixed);
	ioctl(cp.video.descriptor, FBIOGET_VSCREENINFO, &cp.video.mode.original);

	memcpy(&cp.video.mode.current, &cp.video.mode.original, sizeof(struct fb_var_screeninfo));
	printf("Modes:\n");
	printf("\t%i: %dx%d, %dbpp\n", 0, cp.video.mode.original.xres, cp.video.mode.original.yres, cp.video.mode.original.bits_per_pixel);
	for(i = 1; i < sizeof(modes) / sizeof(MODE); i++)
		printf("\t%i: %dx%d, %dbpp\n", i, modes[i].xres, modes[i].yres, modes[i].bits_per_pixel);
	printf("Mode: ");
	scanf("%i", &cp.video.mode.code);
	getchar();

	if(cp.video.mode.code)
	{
		cp.video.mode.current.xres = modes[cp.video.mode.code].xres;
		cp.video.mode.current.yres = modes[cp.video.mode.code].yres;
		cp.video.mode.current.bits_per_pixel = modes[cp.video.mode.code].bits_per_pixel;
		ioctl(cp.video.descriptor, FBIOPUT_VSCREENINFO, &cp.video.mode.current);
	}
	switch(cp.video.mode.current.bits_per_pixel >> 3)
	{
		case 1:
		{
			SCR_SetRGBPalette();
			SCR_SetPixelRGB = SCR_SetPixelRGB8;
			SCR_GetPixelRGB = SCR_GetPixelRGB8;
			SCR_SetBoxRGB = SCR_SetBoxRGB8;
			SCR_SetBox = SCR_SetBoxRGB8;
			BOX_SetPixelRGB = BOX_SetPixelRGB8;
			BOX_GetPixelRGB = BOX_GetPixelRGB8;
			break;
		}
		case 2:
		{
			SCR_SetPixelRGB = SCR_SetPixelRGB16;
			SCR_GetPixelRGB = SCR_GetPixelRGB16;
			SCR_SetBoxRGB = SCR_SetBoxRGB16;
			SCR_SetBox = SCR_SetBoxRGB16;
			BOX_SetPixelRGB = BOX_SetPixelRGB16;
			BOX_GetPixelRGB = BOX_GetPixelRGB16;
			break;
		}
		case 3:
		{
			SCR_SetPixelRGB = SCR_SetPixelRGB24;
			SCR_GetPixelRGB = SCR_GetPixelRGB24;
			SCR_SetBoxRGB = SCR_SetBoxRGB24;
			SCR_SetBox = SCR_SetBoxRGB24;
			BOX_SetPixelRGB = BOX_SetPixelRGB24;
			BOX_GetPixelRGB = BOX_GetPixelRGB24;
			break;
		}
		case 4:
		{
			SCR_SetPixelRGB = SCR_SetPixelRGB32;
			SCR_GetPixelRGB = SCR_GetPixelRGB32;
			SCR_SetBoxRGB = SCR_SetBoxRGB32;
			SCR_SetBox = SCR_SetBoxRGBA;
			BOX_SetPixelRGB = BOX_SetPixelRGB32;
			BOX_GetPixelRGB = BOX_GetPixelRGB32;
			break;
		}
	}
	cp.video.screen = mmap(NULL, cp.video.mode.fixed.smem_len, PROT_READ | PROT_WRITE, MAP_SHARED, cp.video.descriptor, 0);

	SWP_Init();
}

void
SCR_Final(void)
{
	SWP_Final();

	munmap(cp.video.screen, cp.video.mode.fixed.smem_len);

	ioctl(cp.video.descriptor, FBIOPUT_VSCREENINFO, &cp.video.mode.original);
    	close(cp.video.descriptor);
}

void
SCR_GetSize(unsigned int *width, unsigned int *height)
{
	*width = cp.video.mode.current.xres;
	*height = cp.video.mode.current.yres;
}

void
SCR_GetDepth(unsigned int *depth)
{
	*depth = cp.video.mode.current.bits_per_pixel >> 3;
}

void
SCR_Clear(void)
{
	memset(cp.video.screen, 0, cp.video.mode.fixed.smem_len);
	/*
	unsigned int i, j;
	for(j = 0; j < cp.video.mode.current.yres; j++)
	{
		for(i = 0; i < cp.video.mode.current.xres; i++)
		{
			switch((cp.video.mode.current.bits_per_pixel >> 3))
			{
				case 1:
				{
					SCR_SetPixelColor(i, j, 0);
					break;
				}
				case 2:
				{
					SCR_SetPixelRGB16(i, j, 0, 0, 0);
					break;
				}
				case 3:
				{
					SCR_SetPixelRGB24(i, j, 0, 0, 0);
					break;
				}
				case 4:
				{
					SCR_SetPixelRGBA(i, j, 0, 0, 0, 0);
					break;
				}
			}
		}
	}
	*/
}

void
SCR_Swap(void)
{
	memcpy(cp.video.screen, cp.video.swap, cp.video.mode.fixed.smem_len);
	//usleep(10000);
}

void
SCR_SetPalette(unsigned short pal[4][256])
{
	cp.video.mode.palette.start = 0;
	cp.video.mode.palette.len = 256;
	cp.video.mode.palette.red = pal[0];
	cp.video.mode.palette.green = pal[1];
	cp.video.mode.palette.blue = pal[2];
	cp.video.mode.palette.transp = pal[3];
	ioctl(cp.video.descriptor, FBIOPUTCMAP, &cp.video.mode.palette);
}

void
SCR_SetRGBPalette(void)
{
	// [rrgggbbb];
	unsigned short pal[4][256];
	unsigned char c;
	for(c = 0; c < 256; c++)
	{
		/* gl_setrgbpalette
		pal[0][c] = ((c & 192) >> 6) * (64 / 4);
		pal[1][c] = ((c & 56) >> 3) * (64 / 8);
		pal[2][c] = (c & 7) * (64 / 8);
		pal[3][c] = 0;
		*/
		pal[0][c] = ((c & 0xC0) >> 6) << 6;
		pal[1][c] = ((c & 0x38) >> 3) << 5;
		pal[2][c] = (c & 0x07) << 5;
		pal[3][c] = 0;
	}
	SCR_SetPalette(pal);
}

void
SCR_SetPixelColor(int x, int y, unsigned char c)
{
	if(0 <= x && x < cp.video.mode.current.xres && 0 <= y && y < cp.video.mode.current.yres)
 		*(cp.video.screen + y * cp.video.mode.fixed.line_length + x) = c;
}

void
SCR_SetPixelRGB8(int x, int y, unsigned char r, unsigned char g, unsigned char b)
{
	if(0 <= x && x < cp.video.mode.current.xres && 0 <= y && y < cp.video.mode.current.yres)
 		*(cp.video.screen + y * cp.video.mode.fixed.line_length + x) = (r >> 6) << 6 | (g >> 5) << 3 | b >> 5;
}

void
SCR_SetPixelRGB16(int x, int y, unsigned char r, unsigned char g, unsigned char b)
{
	if(0 <= x && x < cp.video.mode.current.xres && 0 <= y && y < cp.video.mode.current.yres)
		*(unsigned short *)(cp.video.screen + (y * cp.video.mode.fixed.line_length + x * (cp.video.mode.current.bits_per_pixel >> 3))) = (r << 11) | (g << 5) | (b);
}

void
SCR_SetPixelRGB24(int x, int y, unsigned char r, unsigned char g, unsigned char b)
{
	if(0 <= x && x < cp.video.mode.current.xres && 0 <= y && y < cp.video.mode.current.yres)
	{
		*(cp.video.screen + y * cp.video.mode.fixed.line_length + x * (cp.video.mode.current.bits_per_pixel >> 3) + 0) = b;
		*(cp.video.screen + y * cp.video.mode.fixed.line_length + x * (cp.video.mode.current.bits_per_pixel >> 3) + 1) = g;
		*(cp.video.screen + y * cp.video.mode.fixed.line_length + x * (cp.video.mode.current.bits_per_pixel >> 3) + 2) = r;
	}
}

void
SCR_SetPixelRGB32(int x, int y, unsigned char r, unsigned char g, unsigned char b)
{
	if(0 <= x && x < cp.video.mode.current.xres && 0 <= y && y < cp.video.mode.current.yres)
	{
		*(cp.video.screen + y * cp.video.mode.fixed.line_length + x * (cp.video.mode.current.bits_per_pixel >> 3) + 0) = b;
		*(cp.video.screen + y * cp.video.mode.fixed.line_length + x * (cp.video.mode.current.bits_per_pixel >> 3) + 1) = g;
		*(cp.video.screen + y * cp.video.mode.fixed.line_length + x * (cp.video.mode.current.bits_per_pixel >> 3) + 2) = r;
	}
}

void
SCR_SetPixelRGBA(int x, int y, unsigned char r, unsigned char g, unsigned char b, unsigned char a)
{
	if(0 <= x && x < cp.video.mode.current.xres && 0 <= y && y < cp.video.mode.current.yres)
	{
		*(cp.video.screen + y * cp.video.mode.fixed.line_length + x * (cp.video.mode.current.bits_per_pixel >> 3) + 0) = b;
		*(cp.video.screen + y * cp.video.mode.fixed.line_length + x * (cp.video.mode.current.bits_per_pixel >> 3) + 1) = g;
		*(cp.video.screen + y * cp.video.mode.fixed.line_length + x * (cp.video.mode.current.bits_per_pixel >> 3) + 2) = r;
		*(cp.video.screen + y * cp.video.mode.fixed.line_length + x * (cp.video.mode.current.bits_per_pixel >> 3) + 3) = a;
	}
}

void
SCR_GetPixelColor(int x, int y, unsigned char *c)
{
	if(0 <= x && x < cp.video.mode.current.xres && 0 <= y && y < cp.video.mode.current.yres)
 		*c = *(cp.video.screen + y * cp.video.mode.fixed.line_length + x);
}

void
SCR_GetPixelRGB8(int x, int y, unsigned char *r, unsigned char *g, unsigned char *b)
{
	if(0 <= x && x < cp.video.mode.current.xres && 0 <= y && y < cp.video.mode.current.yres)
	{
		unsigned char c = *(cp.video.screen + y * cp.video.mode.fixed.line_length + x);
		*r = ((c & 0xC0) >> 6) << 6;
		*g = ((c & 0x38) >> 3) << 5;
		*b = (c & 0x07) << 5;
	}
}

void
SCR_GetPixelRGB16(int x, int y, unsigned char *r, unsigned char *g, unsigned char *b)
{
	if(0 <= x && x < cp.video.mode.current.xres && 0 <= y && y < cp.video.mode.current.yres)
	{
		unsigned short p = *(unsigned short *)(cp.video.screen + (y * cp.video.mode.fixed.line_length + x * (cp.video.mode.current.bits_per_pixel >> 3)));
		*r = ((p & 0xF800) >> 11) << 3;
		*g = ((p & 0x07E0) >> 5) << 2;
		*b = (p & 0x001F) << 3;
	}
}

void
SCR_GetPixelRGB24(int x, int y, unsigned char *r, unsigned char *g, unsigned char *b)
{
	if(0 <= x && x < cp.video.mode.current.xres && 0 <= y && y < cp.video.mode.current.yres)
	{
		*b = *(cp.video.screen + y * cp.video.mode.fixed.line_length + x * (cp.video.mode.current.bits_per_pixel >> 3) + 0);
		*g = *(cp.video.screen + y * cp.video.mode.fixed.line_length + x * (cp.video.mode.current.bits_per_pixel >> 3) + 1);
		*r = *(cp.video.screen + y * cp.video.mode.fixed.line_length + x * (cp.video.mode.current.bits_per_pixel >> 3) + 2);
	}
}

void
SCR_GetPixelRGB32(int x, int y, unsigned char *r, unsigned char *g, unsigned char *b)
{
	if(0 <= x && x < cp.video.mode.current.xres && 0 <= y && y < cp.video.mode.current.yres)
	{
		*b = *(cp.video.screen + y * cp.video.mode.fixed.line_length + x * (cp.video.mode.current.bits_per_pixel >> 3) + 0);
		*g = *(cp.video.screen + y * cp.video.mode.fixed.line_length + x * (cp.video.mode.current.bits_per_pixel >> 3) + 1);
		*r = *(cp.video.screen + y * cp.video.mode.fixed.line_length + x * (cp.video.mode.current.bits_per_pixel >> 3) + 2);
	}
}

void
SCR_GetPixelRGBA(int x, int y, unsigned char *r, unsigned char *g, unsigned char *b, unsigned char *a)
{
	if(0 <= x && x < cp.video.mode.current.xres && 0 <= y && y < cp.video.mode.current.yres)
	{
		*b = *(cp.video.screen + y * cp.video.mode.fixed.line_length + x * (cp.video.mode.current.bits_per_pixel >> 3) + 0);
		*g = *(cp.video.screen + y * cp.video.mode.fixed.line_length + x * (cp.video.mode.current.bits_per_pixel >> 3) + 1);
		*r = *(cp.video.screen + y * cp.video.mode.fixed.line_length + x * (cp.video.mode.current.bits_per_pixel >> 3) + 2);
		*a = *(cp.video.screen + y * cp.video.mode.fixed.line_length + x * (cp.video.mode.current.bits_per_pixel >> 3) + 3);
	}
}

void
SCR_SetBoxColor(int x, int y, BOX *box)
{
	unsigned int i, j;
	unsigned char c;
	for(j = 0; j < box->info.height; j++)
	{
		for(i = 0; i < box->info.width; i++)
		{
			BOX_GetPixelColor(box, i, j, &c);
			SCR_SetPixelColor(x + i, y + j, c);
		}
	}
}

void
SCR_SetBoxRGB8(int x, int y, BOX *box)
{
	unsigned int i, j;
	unsigned char r, g, b;
	for(j = 0; j < box->info.height; j++)
	{
		for(i = 0; i < box->info.width; i++)
		{
			BOX_GetPixelRGB8(box, i, j, &r, &g, &b);
			SCR_SetPixelRGB8(x + i, y + j, r, g, b);
		}
	}
}

void
SCR_SetBoxRGB16(int x, int y, BOX *box)
{
	unsigned int i, j;
	unsigned char r, g, b;
	for(j = 0; j < box->info.height; j++)
	{
		for(i = 0; i < box->info.width; i++)
		{
			BOX_GetPixelRGB16(box, i, j, &r, &g, &b);
			SCR_SetPixelRGB16(x + i, y + j, r, g, b);
		}
	}
}

void
SCR_SetBoxRGB24(int x, int y, BOX *box)
{
	unsigned int i, j;
	unsigned char r, g, b;
	for(j = 0; j < box->info.height; j++)
	{
		for(i = 0; i < box->info.width; i++)
		{
			BOX_GetPixelRGB24(box, i, j, &r, &g, &b);
			SCR_SetPixelRGB24(x + i, y + j, r, g, b);
		}
	}
}

void
SCR_SetBoxRGB32(int x, int y, BOX *box)
{
	unsigned int i, j;
	unsigned char r, g, b;
	for(j = 0; j < box->info.height; j++)
	{
		for(i = 0; i < box->info.width; i++)
		{
			BOX_GetPixelRGB32(box, i, j, &r, &g, &b);
			SCR_SetPixelRGB32(x + i, y + j, r, g, b);
		}
	}
}

void
SCR_SetBoxRGBA(int x, int y, BOX *box)
{
	unsigned int i, j;
	unsigned char r, g, b, a;
	for(j = 0; j < box->info.height; j++)
	{
		for(i = 0; i < box->info.width; i++)
		{
			BOX_GetPixelRGBA(box, i, j, &r, &g, &b, &a);
			SCR_SetPixelRGBA(x + i, y + j, r, g, b, a);
		}
	}
}

void
SCR_SetImage(char *image)
{
	MagickWand *mw;
	unsigned char *buffer;
	unsigned int i, j;
	buffer = malloc(cp.video.mode.current.xres * cp.video.mode.current.yres * 4);
	mw = NewMagickWand();
	MagickReadImage(mw, image);
	if(cp.video.mode.current.xres != MagickGetImageWidth(mw) || cp.video.mode.current.yres != MagickGetImageHeight(mw))
		MagickScaleImage(mw, cp.video.mode.current.xres, cp.video.mode.current.yres);
	MagickExportImagePixels(mw, 0, 0, cp.video.mode.current.xres, cp.video.mode.current.yres, "RGBA", CharPixel, buffer);
	DestroyMagickWand(mw);
	for(j = 0; j < cp.video.mode.current.yres; j++)
	{
		for(i = 0; i < cp.video.mode.current.xres; i++)
		{
			switch((cp.video.mode.current.bits_per_pixel >> 3))
			{
				case 1:
				{
					SCR_SetPixelColor(i, j, buffer[(j * cp.video.mode.current.xres + i) * 4]);
					break;
				}
				case 2:
				{
					SCR_SetPixelRGB16(i, j, buffer[(j * cp.video.mode.current.xres + i) * 4 + 0], buffer[(j * cp.video.mode.current.xres + i) * 4 + 1] , buffer[(j * cp.video.mode.current.xres + i) * 4 + 2]);
					break;
				}
				case 3:
				{
					SCR_SetPixelRGB24(i, j, buffer[(j * cp.video.mode.current.xres + i) * 4 + 0], buffer[(j * cp.video.mode.current.xres + i) * 4 + 1] , buffer[(j * cp.video.mode.current.xres + i) * 4 + 2]);
					break;
				}
				case 4:
				{
					SCR_SetPixelRGBA(i, j, buffer[(j * cp.video.mode.current.xres + i) * 4 + 0], buffer[(j * cp.video.mode.current.xres + i) * 4 + 1] , buffer[(j * cp.video.mode.current.xres + i) * 4 + 2], buffer[(j * cp.video.mode.current.xres + i) * 4 + 3]);
					break;
				}
			}
		}
	}
	free(buffer);
}

void
SCR_GetImage(char *image)
{
	MagickWand *mw;
	unsigned char *buffer;
	unsigned int i, j;
	unsigned char c, r, g, b, a;
	buffer = malloc(cp.video.mode.current.xres * cp.video.mode.current.yres * 4);
	for(j = 0; j < cp.video.mode.current.yres; j++)
	{
		for(i = 0; i < cp.video.mode.current.xres; i++)
		{
			switch((cp.video.mode.current.bits_per_pixel >> 3))
			{
				case 1:
				{
					SCR_GetPixelColor(i, j, &c);
					buffer[(j * cp.video.mode.current.xres + i) * 4 + 0] = c;
					buffer[(j * cp.video.mode.current.xres + i) * 4 + 1] = c;
					buffer[(j * cp.video.mode.current.xres + i) * 4 + 2] = c;
					buffer[(j * cp.video.mode.current.xres + i) * 4 + 3] = 0;
					break;
				}
				case 2:
				{
					SCR_GetPixelRGB16(i, j, &r, &g, &b);
					buffer[(j * cp.video.mode.current.xres + i) * 4 + 0] = r;
					buffer[(j * cp.video.mode.current.xres + i) * 4 + 1] = g;
					buffer[(j * cp.video.mode.current.xres + i) * 4 + 2] = b;
					buffer[(j * cp.video.mode.current.xres + i) * 4 + 3] = 0;
					break;
				}
				case 3:
				{
					SCR_GetPixelRGB24(i, j, &r, &g, &b);
					buffer[(j * cp.video.mode.current.xres + i) * 4 + 0] = r;
					buffer[(j * cp.video.mode.current.xres + i) * 4 + 1] = g;
					buffer[(j * cp.video.mode.current.xres + i) * 4 + 2] = b;
					buffer[(j * cp.video.mode.current.xres + i) * 4 + 3] = 0;
					break;
				}
				case 4:
				{
					SCR_GetPixelRGBA(i, j, &r, &g, &b, &a);
					buffer[(j * cp.video.mode.current.xres + i) * 4 + 0] = r;
					buffer[(j * cp.video.mode.current.xres + i) * 4 + 1] = g;
					buffer[(j * cp.video.mode.current.xres + i) * 4 + 2] = b;
					buffer[(j * cp.video.mode.current.xres + i) * 4 + 3] = a;
					break;
				}
			}
		}
	}
	mw = NewMagickWand();
	MagickConstituteImage(mw, cp.video.mode.current.xres, cp.video.mode.current.yres, "RGBO", CharPixel, buffer);
	free(buffer);
	MagickWriteImage(mw, image);
	DestroyMagickWand(mw);
}
